// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_

#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <svo/feature_detection.h>
#include <svo/matcher.h>

namespace svo {

	class Frame;
	class Feature;
	class Point;

	/// A seed is a probabilistic depth estimate for a single pixel.
	struct Seed
	{
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		static int batch_counter;
		static int seed_counter;
		int batch_id;                //!< Batch id is the id of the keyframe for which the seed was created.
		int id;                      //!< Seed ID, only used for visualization.
		Feature* ftr;                //!< Feature in the keyframe for which the depth should be computed.
		float a;                     //!< a of Beta distribution: When high, probability of inlier is large.
		float b;                     //!< b of Beta distribution: When high, probability of outlier is large.
		float mu;                    //!< Mean of normal distribution.
		float z_range;               //!< Max range of the possible depth.
		float sigma2;                //!< Variance of normal distribution.
		Matrix2d patch_cov;          //!< Patch covariance in reference image.
		Seed(Feature* ftr, float depth_mean, float depth_min);
	};

	/// Depth filter implements the Bayesian Update proposed in:
	/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández.
	/// In Image and Vision Computing, 29(7):434-441, 2011.
	///
	/// The class uses a callback mechanism such that it can be used also by other
	/// algorithms than nslam and for simplified testing.
	class DepthFilter
	{
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

			typedef boost::unique_lock<boost::mutex> lock_t;
		typedef boost::function<void(Point*, double)> callback_t;

		/// Depth-filter config parameters
		struct Options
		{
			bool check_ftr_angle;                       //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
			bool epi_search_1d;                         //!< restrict Gauss Newton in the epipolar search to the epipolar line.
			bool verbose;                               //!< display output.
			bool use_photometric_disparity_error;       //!< use photometric disparity error instead of 1px error in tau computation.
			int max_n_kfs;                              //!< maximum number of keyframes for which we maintain seeds.
			double sigma_i_sq;                          //!< image noise.
			double seed_convergence_sigma2_thresh;      //!< threshold on depth uncertainty for convergence.
			Options()
				: check_ftr_angle(false),
				epi_search_1d(false),
				verbose(false),
				use_photometric_disparity_error(false),
				max_n_kfs(3),
				sigma_i_sq(5e-4),
				seed_convergence_sigma2_thresh(200.0)
			{}
		} options_;

		//深度滤波器的初始化
		//输入：	feature				feature_detector
		//			seed_converged_cb	一个函数指针，在此之前，FrameHandlerMono::initialize()函数已经将seed_converged_cb绑定到
		//								地图中候选关键点的成员函数newCandidatePoint()
		DepthFilter(
			feature_detection::DetectorPtr feature_detector,
			callback_t seed_converged_cb);

		virtual ~DepthFilter();

		/// Start this thread when seed updating should be in a parallel thread.
		void startThread();

		/// Stop the parallel thread that is running.
		void stopThread();

		/// Add frame to the queue to be processed.
		void addFrame(FramePtr frame);

		/// Add new keyframe to the queue
		void addKeyframe(FramePtr frame, double depth_mean, double depth_min);

		/// Remove all seeds which are initialized from the specified keyframe. This
		/// function is used to make sure that no seeds points to a non-existent frame
		/// when a frame is removed from the map.
		void removeKeyframe(FramePtr frame);

		/// If the map is reset, call this function such that we don't have pointers
		/// to old frames.
		void reset();

		/// Returns a copy of the seeds belonging to frame. Thread-safe.
		/// Can be used to compute the Next-Best-View in parallel.
		/// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
		/// so it is not being deleted while you use it.
		void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);

		/// Return a reference to the seeds. This is NOT THREAD SAFE!
		std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }

		/// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
		static void updateSeed(
			const float x,
			const float tau2,
			Seed* seed);

		/// Compute the uncertainty of the measurement.
		static double computeTau(
			const SE3& T_ref_cur,
			const Vector3d& f,
			const double z,
			const double px_error_angle);

	protected:
		feature_detection::DetectorPtr feature_detector_;
		callback_t seed_converged_cb_;
		std::list<Seed, aligned_allocator<Seed> > seeds_;	//存储所有的种子list
		boost::mutex seeds_mut_;
		bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
		boost::thread* thread_;					//在initialize()函数中被赋值开启线程，在创建FrameHandlerMono对象对赋值具体相机参数时调用initialize()函数
		std::queue<FramePtr> frame_queue_;
		boost::mutex frame_queue_mut_;
		boost::condition_variable frame_queue_cond_;
		FramePtr new_keyframe_;               //!< Next keyframe to extract new seeds.
		bool new_keyframe_set_;               //!< Do we have a new keyframe to process?.
		double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
		double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
		vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
		Matcher matcher_;

		/// Initialize new seeds from a frame.
		void initializeSeeds(FramePtr frame);

		/// Update all seeds with a new measurement frame.
		virtual void updateSeeds(FramePtr frame);

		/// When a new keyframe arrives, the frame queue should be cleared.
		void clearFrameQueue();

		/// A thread that is continuously updating the seeds.
		void updateSeedsLoop();
	};

} // namespace svo

#endif // SVO_DEPTH_FILTER_H_
